﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using ABB.Robotics.Controllers;
using ABB.Robotics.Controllers.Discovery;
using ABB.Robotics.Controllers.RapidDomain;
using ABB.Robotics.Controllers.IOSystemDomain;
using ABB.Robotics.Controllers.MotionDomain;
using System.Net.Sockets;
using System.Threading;
using System.Net;
using System.Timers;
using System.IO;

namespace ABB二次开发
{
    public partial class ABB机器人 : Form
    {
        public ABB机器人()
        {
            InitializeComponent();
            System.Windows.Forms.Control.CheckForIllegalCrossThreadCalls = false;
            sv.MW = this;

            RunThread = new Thread(new ThreadStart(RunData));
            RunThread.IsBackground = true;
            RunThread.Start();


            scanner = new NetworkScanner();//创建实例

            //添加上线监控
            if (this.netWatcher != null)          //
            {
                this.netWatcher.Dispose();      //
                this.netWatcher = null;
            }
            this.netWatcher = new NetworkWatcher(scanner.Controllers);       //watching controller
            this.netWatcher.Found += new EventHandler<NetworkWatcherEventArgs>(HandleFoundEvent);   //上线事件
            this.netWatcher.Lost += new EventHandler<NetworkWatcherEventArgs>(HandleLostEvent);     //掉线事件
            this.netWatcher.EnableRaisingEvents = true;         //激活事件


            HandleFoundEvent(new object(),new NetworkWatcherEventArgs());//初始化数据


            try
            {
                FileStream fs = new FileStream("test.txt", FileMode.Open);
                
            }
            catch (Exception)
            {
                Ts1.Enabled = false;
                Ts2.Enabled = false;
            }
        }
        NetworkScanner scanner;
        private NetworkWatcher netWatcher = null;
        Server sv = new Server();
        private Thread RunThread;
        bool Online = false;
        bool IOFlg = false;
        bool GetBool = false;
        void RunData()//后台数据读取
        {
            bool flg = false;
            int Addn = 0;
            while (true)
            {
                try
                {
                    if (connectFlg && (!GetBool))   //autoFlg&& 
                    {
                        try
                        {
                            //RobTarget PointPick = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                            //RobTarget a1 = PointPick;
                            RapidData GetImage = controller.Rapid.GetRapidData("T_ROB1", "START");
                            Bool GetImageFlg = (Bool)GetImage.Value;
                            if (IOFlg == false && GetImageFlg == true)
                            {
                                GetImages();//拍照
                            }
                            IOFlg = GetImageFlg;
                        }
                        catch
                        {
                        }



                        if (autoFlg)
                        {
                            Addn++;
                            if (Addn > 18)
                            {
                                Addn = 0;
                                try
                                {
                                    if (flg)
                                    {
                                        using (Mastership m = Mastership.Request(controller.Rapid))
                                        {
                                            RapidData HEARTFlg = controller.Rapid.GetRapidData("T_ROB1", "VISION_HEART");//写心跳标志
                                            HEARTFlg.Value = new Bool(true);
                                            // Bool HeartFlg = (Bool)HEARTFlg.Value;
                                        }
                                    }
                                    else
                                    {
                                        using (Mastership m = Mastership.Request(controller.Rapid))
                                        {
                                            RapidData HEARTFlg = controller.Rapid.GetRapidData("T_ROB1", "VISION_HEART");//写心跳标志
                                            HEARTFlg.Value = new Bool(false);
                                            // Bool HeartFlg = (Bool)HEARTFlg.Value;
                                        }
                                    }

                                }
                                catch { }
                                flg = !flg;
                            }

                        }
                    }
                    Thread.Sleep(80);
                }
                catch
                {}             
            }
        }
        public void GetImages()//获取图片
        {
            if (connectFlg)   //autoFlg&& 
            {
                int i = 0;
                for (; i < reCount; i++)
                {
                    try
                    {
                        RapidData GetImage = controller.Rapid.GetRapidData("T_ROB1", "car_style");
                        Num num = (Num)GetImage.Value;
                        RapidData GetImage1 = controller.Rapid.GetRapidData("T_ROB1", "reg_take_picture_fedback");
                        Num num1 = (Num)GetImage1.Value;

                        if (num1==1)
                        {
                            SetString("拍照");
                        }

                        RobTarget jCurPos = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Base);
                        string send = jCurPos.Trans.X.ToString() + "," +
                            jCurPos.Trans.Y.ToString() + "," +
                            jCurPos.Trans.Z.ToString() + "," +
                            jCurPos.Rot.Q1.ToString() + "," +
                            jCurPos.Rot.Q2.ToString() + "," +
                            jCurPos.Rot.Q3.ToString() + "," +
                            jCurPos.Rot.Q4.ToString();

                        sv.SendStr("get" + "," + num.ToString() + "," + num1.ToString() + "," + send);
                        if (num1==1)
                        {
                            FirstCap.BackColor = Color.Green;
                            SendComd.BackColor = Color.Red;
                            SeconCap.BackColor = Color.Red;
                        }
                        if (num1 == 2)
                        {
                            SeconCap.BackColor = Color.Green;
                        }
                        return;
                    }
                    catch
                    {
                        SetString("获取车型 获取位置失败 重复: "+i.ToString());
                    }
                    Thread.Sleep(10);
                }
                if (i== reCount)
                {
                    SetString("获取车型 获取位置完全失败放弃获取");
                    FirstCap.BackColor = Color.Red;
                    SendComd.BackColor = Color.Red;
                    SeconCap.BackColor = Color.Red;
                }

            }   
        }

        /// ///////////////////////////////////////////////////测试//////////////////////////////////////////////////////////////
        int ads = 0;
        private void Ts1_Click(object sender, EventArgs e)
        {
            if (ads % 2 == 1)
            {
                sv.SendStr("get,1,1,0,0,0,0,0,0,0");
            }
            else
            {
                sv.SendStr("get,1,2,0,0,0,0,0,0,0");
            }
            ads++;
        }
        private void tm_Tick(object sender, EventArgs e)
        {
            if (ads % 2 == 1)
            {
                sv.SendStr("get,1,1,0,0,0,0,0,0,0");
            }
            else
            {
                sv.SendStr("get,1,2,0,0,0,0,0,0,0");
            }
            ads++;
        }
        bool tmt = false;
        private void Ts2_Click(object sender, EventArgs e)
        {
            if (!tmt)
            {
                tm.Start();
                tmt = true;
            }
            else
            {
                tm.Stop();
                tmt = false;
            }
        }

        int reCount = 7;//通信重复次数

        //读数据 机器人坐标
        public string ret(string str)
        {
            if (controller==null)
            {
                return "close";
            }

            for (int i = 0; i < reCount; i++)
            {
                try
                {
                    RobTarget jCurPos = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Base);
                    string send = jCurPos.Trans.X.ToString() + "," +
                        jCurPos.Trans.Y.ToString() + "," +
                        jCurPos.Trans.Z.ToString() + "," +
                        jCurPos.Rot.Q1.ToString() + "," +
                        jCurPos.Rot.Q2.ToString() + "," +
                        jCurPos.Rot.Q3.ToString() + "," +
                        jCurPos.Rot.Q4.ToString();
                    return send;
                }
                catch{ SetString("获取机器人坐标失败 重复: " + i.ToString()); }
                Thread.Sleep(5);
            }
            return "error";
        }

        //显示结果
        public void SetString(string str)
        {

            string strc = DateTime.Now.ToLocalTime().ToString() + " :  " + str+"\r\n";
            DispInfo.AppendText(strc);
        }

        //写入机器人数据 坐标
        public void RunPoint(string str)
        {

            GetBool = true;
            string[] strarr = str.Split(',');
            if (strarr.Length>=44)
            {
                SetString("结果： " + strarr[42] + "  状态：" + strarr[43]);
            }
           
            if (!connectFlg)
            {
                //MessageBox.Show("机器人故障无法连接");
                SetString("机器人故障无法连接");
                return;
            }

            if (strarr.Length==15)
            {

                try
                {
                    RobTarget Point = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                    RobTarget PointAdd = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);

                    Point.Trans.X = float.Parse(strarr[0]);
                    Point.Trans.Y = float.Parse(strarr[1]);
                    Point.Trans.Z = float.Parse(strarr[2]);
                    Point.Rot.Q1 = float.Parse(strarr[3]);
                    Point.Rot.Q2 = float.Parse(strarr[4]);
                    Point.Rot.Q3 = float.Parse(strarr[5]);
                    Point.Rot.Q4 = float.Parse(strarr[6]);

                    PointAdd.Trans.X = float.Parse(strarr[7]);
                    PointAdd.Trans.Y = float.Parse(strarr[8]);
                    PointAdd.Trans.Z = float.Parse(strarr[9]);
                    PointAdd.Rot.Q1 = float.Parse(strarr[10]);
                    PointAdd.Rot.Q2 = float.Parse(strarr[11]);
                    PointAdd.Rot.Q3 = float.Parse(strarr[12]);
                    PointAdd.Rot.Q4 = float.Parse(strarr[13]);
                    switch (strarr[14])
                    {
                        case "pick":
                            using (Mastership m = Mastership.Request(controller.Rapid))
                            {
                                RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "pick");
                                pick.Value = Point;

                                RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up");
                                pick_z_up.Value = PointAdd;
                            }
                            break;
                        case "put":
                            using (Mastership m = Mastership.Request(controller.Rapid))
                            {
                                RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "put");
                                pick.Value = Point;

                                RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pre_put");
                                pick_z_up.Value = PointAdd;
                            }
                            break;
                        default:
                            break;
                    }
                }
                catch
                {
                    MessageBox.Show("手动向机器人写入数据失败");
                }
            }

            if (strarr.Length == 44)
            {
                #region old
                //try
                //{
                //    RobTarget PointPick = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //    RobTarget PointPick_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //    RobTarget PointPick_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //    RobTarget PointPut = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //    RobTarget PointPut_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //    RobTarget PointPut_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);

                //   // RobTarget k= PointPut_upup.

                //    PointPick.Trans.X = float.Parse(strarr[0]);
                //    PointPick.Trans.Y = float.Parse(strarr[1]);
                //    PointPick.Trans.Z = float.Parse(strarr[2]);
                //    PointPick.Rot.Q1 = float.Parse(strarr[3]);
                //    PointPick.Rot.Q2 = float.Parse(strarr[4]);
                //    PointPick.Rot.Q3 = float.Parse(strarr[5]);
                //    PointPick.Rot.Q4 = float.Parse(strarr[6]);

                //    PointPick_up.Trans.X = float.Parse(strarr[7]);
                //    PointPick_up.Trans.Y = float.Parse(strarr[8]);
                //    PointPick_up.Trans.Z = float.Parse(strarr[9]);
                //    PointPick_up.Rot.Q1 = float.Parse(strarr[10]);
                //    PointPick_up.Rot.Q2 = float.Parse(strarr[11]);
                //    PointPick_up.Rot.Q3 = float.Parse(strarr[12]);
                //    PointPick_up.Rot.Q4 = float.Parse(strarr[13]);

                //    PointPick_upup.Trans.X = float.Parse(strarr[14]);
                //    PointPick_upup.Trans.Y = float.Parse(strarr[15]);
                //    PointPick_upup.Trans.Z = float.Parse(strarr[16]);
                //    PointPick_upup.Rot.Q1 = float.Parse(strarr[17]);
                //    PointPick_upup.Rot.Q2 = float.Parse(strarr[18]);
                //    PointPick_upup.Rot.Q3 = float.Parse(strarr[19]);
                //    PointPick_upup.Rot.Q4 = float.Parse(strarr[20]);

                //    PointPut.Trans.X = float.Parse(strarr[21]);
                //    PointPut.Trans.Y = float.Parse(strarr[22]);
                //    PointPut.Trans.Z = float.Parse(strarr[23]);
                //    PointPut.Rot.Q1 = float.Parse(strarr[24]);
                //    PointPut.Rot.Q2 = float.Parse(strarr[25]);
                //    PointPut.Rot.Q3 = float.Parse(strarr[26]);
                //    PointPut.Rot.Q4 = float.Parse(strarr[27]);

                //    PointPut_up.Trans.X = float.Parse(strarr[28]);
                //    PointPut_up.Trans.Y = float.Parse(strarr[29]);
                //    PointPut_up.Trans.Z = float.Parse(strarr[30]);
                //    PointPut_up.Rot.Q1 = float.Parse(strarr[31]);
                //    PointPut_up.Rot.Q2 = float.Parse(strarr[32]);
                //    PointPut_up.Rot.Q3 = float.Parse(strarr[33]);
                //    PointPut_up.Rot.Q4 = float.Parse(strarr[34]);

                //    PointPut_upup.Trans.X = float.Parse(strarr[35]);
                //    PointPut_upup.Trans.Y = float.Parse(strarr[36]);
                //    PointPut_upup.Trans.Z = float.Parse(strarr[37]);
                //    PointPut_upup.Rot.Q1 = float.Parse(strarr[38]);
                //    PointPut_upup.Rot.Q2 = float.Parse(strarr[39]);
                //    PointPut_upup.Rot.Q3 = float.Parse(strarr[40]);
                //    PointPut_upup.Rot.Q4 = float.Parse(strarr[41]);

                //    using (Mastership m = Mastership.Request(controller.Rapid))
                //    {
                //        RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "pick");
                //        pick.Value = PointPick;

                //        RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up");
                //        pick_z_up.Value = PointPick_up;

                //        RapidData pick_z_upup = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up_hight");
                //        pick_z_upup.Value = PointPick_upup;

                //        RapidData picka = controller.Rapid.GetRapidData("T_ROB1", "put");
                //        picka.Value = PointPut;

                //        RapidData pick_z_upa = controller.Rapid.GetRapidData("T_ROB1", "pre_put");
                //        pick_z_upa.Value = PointPut_up;

                //        RapidData pick_z_upaup = controller.Rapid.GetRapidData("T_ROB1", "pre_put_hight");
                //        pick_z_upaup.Value = PointPut_upup;

                //        RapidData Flgs = controller.Rapid.GetRapidData("T_ROB1", "VISION_STATE");//
                //        Flgs.Value = new Num(Convert.ToInt32(strarr[43]));

                //        RapidData Flg = controller.Rapid.GetRapidData("T_ROB1", "plug_state");//
                //        Flg.Value = new Num(Convert.ToInt32(strarr[42]));

                //        lbs.Text = DateTime.Now.TimeOfDay.ToString()+"__发送";
                //    }
                //}
                //catch 
                //{
                //    Thread.Sleep(50);
                //    try
                //    {
                //        RobTarget PointPick = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //        RobTarget PointPick_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //        RobTarget PointPick_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //        RobTarget PointPut = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //        RobTarget PointPut_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //        RobTarget PointPut_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);

                //        PointPick.Trans.X = float.Parse(strarr[0]);
                //        PointPick.Trans.Y = float.Parse(strarr[1]);
                //        PointPick.Trans.Z = float.Parse(strarr[2]);
                //        PointPick.Rot.Q1 = float.Parse(strarr[3]);
                //        PointPick.Rot.Q2 = float.Parse(strarr[4]);
                //        PointPick.Rot.Q3 = float.Parse(strarr[5]);
                //        PointPick.Rot.Q4 = float.Parse(strarr[6]);

                //        PointPick_up.Trans.X = float.Parse(strarr[7]);
                //        PointPick_up.Trans.Y = float.Parse(strarr[8]);
                //        PointPick_up.Trans.Z = float.Parse(strarr[9]);
                //        PointPick_up.Rot.Q1 = float.Parse(strarr[10]);
                //        PointPick_up.Rot.Q2 = float.Parse(strarr[11]);
                //        PointPick_up.Rot.Q3 = float.Parse(strarr[12]);
                //        PointPick_up.Rot.Q4 = float.Parse(strarr[13]);

                //        PointPick_upup.Trans.X = float.Parse(strarr[14]);
                //        PointPick_upup.Trans.Y = float.Parse(strarr[15]);
                //        PointPick_upup.Trans.Z = float.Parse(strarr[16]);
                //        PointPick_upup.Rot.Q1 = float.Parse(strarr[17]);
                //        PointPick_upup.Rot.Q2 = float.Parse(strarr[18]);
                //        PointPick_upup.Rot.Q3 = float.Parse(strarr[19]);
                //        PointPick_upup.Rot.Q4 = float.Parse(strarr[20]);

                //        PointPut.Trans.X = float.Parse(strarr[21]);
                //        PointPut.Trans.Y = float.Parse(strarr[22]);
                //        PointPut.Trans.Z = float.Parse(strarr[23]);
                //        PointPut.Rot.Q1 = float.Parse(strarr[24]);
                //        PointPut.Rot.Q2 = float.Parse(strarr[25]);
                //        PointPut.Rot.Q3 = float.Parse(strarr[26]);
                //        PointPut.Rot.Q4 = float.Parse(strarr[27]);

                //        PointPut_up.Trans.X = float.Parse(strarr[28]);
                //        PointPut_up.Trans.Y = float.Parse(strarr[29]);
                //        PointPut_up.Trans.Z = float.Parse(strarr[30]);
                //        PointPut_up.Rot.Q1 = float.Parse(strarr[31]);
                //        PointPut_up.Rot.Q2 = float.Parse(strarr[32]);
                //        PointPut_up.Rot.Q3 = float.Parse(strarr[33]);
                //        PointPut_up.Rot.Q4 = float.Parse(strarr[34]);

                //        PointPut_upup.Trans.X = float.Parse(strarr[35]);
                //        PointPut_upup.Trans.Y = float.Parse(strarr[36]);
                //        PointPut_upup.Trans.Z = float.Parse(strarr[37]);
                //        PointPut_upup.Rot.Q1 = float.Parse(strarr[38]);
                //        PointPut_upup.Rot.Q2 = float.Parse(strarr[39]);
                //        PointPut_upup.Rot.Q3 = float.Parse(strarr[40]);
                //        PointPut_upup.Rot.Q4 = float.Parse(strarr[41]);

                //        using (Mastership m = Mastership.Request(controller.Rapid))
                //        {
                //            RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "pick");
                //            pick.Value = PointPick;

                //            RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up");
                //            pick_z_up.Value = PointPick_up;

                //            RapidData pick_z_upup = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up_hight");
                //            pick_z_upup.Value = PointPick_upup;

                //            RapidData picka = controller.Rapid.GetRapidData("T_ROB1", "put");
                //            picka.Value = PointPut;

                //            RapidData pick_z_upa = controller.Rapid.GetRapidData("T_ROB1", "pre_put");
                //            pick_z_upa.Value = PointPut_up;

                //            RapidData pick_z_upaup = controller.Rapid.GetRapidData("T_ROB1", "pre_put_hight");
                //            pick_z_upaup.Value = PointPut_upup;

                //            RapidData Flgs = controller.Rapid.GetRapidData("T_ROB1", "VISION_STATE");//
                //            Flgs.Value = new Num(Convert.ToInt32(strarr[43]));

                //            RapidData Flg = controller.Rapid.GetRapidData("T_ROB1", "plug_state");//
                //            Flg.Value = new Num(Convert.ToInt32(strarr[42]));

                //            lbs.Text = DateTime.Now.TimeOfDay.ToString() + "__发送_二次确认";
                //        }
                //    }
                //    catch
                //    {
                //        Thread.Sleep(50);
                //        try
                //        {
                //            RobTarget PointPick = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //            RobTarget PointPick_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //            RobTarget PointPick_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //            RobTarget PointPut = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //            RobTarget PointPut_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                //            RobTarget PointPut_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);

                //            PointPick.Trans.X = float.Parse(strarr[0]);
                //            PointPick.Trans.Y = float.Parse(strarr[1]);
                //            PointPick.Trans.Z = float.Parse(strarr[2]);
                //            PointPick.Rot.Q1 = float.Parse(strarr[3]);
                //            PointPick.Rot.Q2 = float.Parse(strarr[4]);
                //            PointPick.Rot.Q3 = float.Parse(strarr[5]);
                //            PointPick.Rot.Q4 = float.Parse(strarr[6]);

                //            PointPick_up.Trans.X = float.Parse(strarr[7]);
                //            PointPick_up.Trans.Y = float.Parse(strarr[8]);
                //            PointPick_up.Trans.Z = float.Parse(strarr[9]);
                //            PointPick_up.Rot.Q1 = float.Parse(strarr[10]);
                //            PointPick_up.Rot.Q2 = float.Parse(strarr[11]);
                //            PointPick_up.Rot.Q3 = float.Parse(strarr[12]);
                //            PointPick_up.Rot.Q4 = float.Parse(strarr[13]);

                //            PointPick_upup.Trans.X = float.Parse(strarr[14]);
                //            PointPick_upup.Trans.Y = float.Parse(strarr[15]);
                //            PointPick_upup.Trans.Z = float.Parse(strarr[16]);
                //            PointPick_upup.Rot.Q1 = float.Parse(strarr[17]);
                //            PointPick_upup.Rot.Q2 = float.Parse(strarr[18]);
                //            PointPick_upup.Rot.Q3 = float.Parse(strarr[19]);
                //            PointPick_upup.Rot.Q4 = float.Parse(strarr[20]);

                //            PointPut.Trans.X = float.Parse(strarr[21]);
                //            PointPut.Trans.Y = float.Parse(strarr[22]);
                //            PointPut.Trans.Z = float.Parse(strarr[23]);
                //            PointPut.Rot.Q1 = float.Parse(strarr[24]);
                //            PointPut.Rot.Q2 = float.Parse(strarr[25]);
                //            PointPut.Rot.Q3 = float.Parse(strarr[26]);
                //            PointPut.Rot.Q4 = float.Parse(strarr[27]);

                //            PointPut_up.Trans.X = float.Parse(strarr[28]);
                //            PointPut_up.Trans.Y = float.Parse(strarr[29]);
                //            PointPut_up.Trans.Z = float.Parse(strarr[30]);
                //            PointPut_up.Rot.Q1 = float.Parse(strarr[31]);
                //            PointPut_up.Rot.Q2 = float.Parse(strarr[32]);
                //            PointPut_up.Rot.Q3 = float.Parse(strarr[33]);
                //            PointPut_up.Rot.Q4 = float.Parse(strarr[34]);

                //            PointPut_upup.Trans.X = float.Parse(strarr[35]);
                //            PointPut_upup.Trans.Y = float.Parse(strarr[36]);
                //            PointPut_upup.Trans.Z = float.Parse(strarr[37]);
                //            PointPut_upup.Rot.Q1 = float.Parse(strarr[38]);
                //            PointPut_upup.Rot.Q2 = float.Parse(strarr[39]);
                //            PointPut_upup.Rot.Q3 = float.Parse(strarr[40]);
                //            PointPut_upup.Rot.Q4 = float.Parse(strarr[41]);

                //            using (Mastership m = Mastership.Request(controller.Rapid))
                //            {
                //                RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "pick");
                //                pick.Value = PointPick;

                //                RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up");
                //                pick_z_up.Value = PointPick_up;

                //                RapidData pick_z_upup = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up_hight");
                //                pick_z_upup.Value = PointPick_upup;

                //                RapidData picka = controller.Rapid.GetRapidData("T_ROB1", "put");
                //                picka.Value = PointPut;

                //                RapidData pick_z_upa = controller.Rapid.GetRapidData("T_ROB1", "pre_put");
                //                pick_z_upa.Value = PointPut_up;

                //                RapidData pick_z_upaup = controller.Rapid.GetRapidData("T_ROB1", "pre_put_hight");
                //                pick_z_upaup.Value = PointPut_upup;

                //                RapidData Flgs = controller.Rapid.GetRapidData("T_ROB1", "VISION_STATE");//
                //                Flgs.Value = new Num(Convert.ToInt32(strarr[43]));

                //                RapidData Flg = controller.Rapid.GetRapidData("T_ROB1", "plug_state");//
                //                Flg.Value = new Num(Convert.ToInt32(strarr[42]));

                //                lbs.Text = DateTime.Now.TimeOfDay.ToString() + "__发送_三次确认";
                //            }
                //        }
                //        catch
                //        {
                //             MessageBox.Show("自动写入坐标失败");
                //        }
                //    }
                //}
                #endregion


                RobTarget PointPick = new RobTarget();
                RobTarget PointPick_up = new RobTarget();
                RobTarget PointPick_upup = new RobTarget();
                RobTarget PointPut = new RobTarget();
                RobTarget PointPut_up = new RobTarget();
                RobTarget PointPut_upup = new RobTarget();
                for (int i = 0; i < reCount; i++)
                {
                    try
                    {
                        PointPick = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        PointPick_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        PointPick_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        PointPut = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        PointPut_up = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        PointPut_upup = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.WorkObject);
                        break;
                    }
                    catch{}
                    Thread.Sleep(1);
                }

                #region 赋值
                PointPick.Trans.X = float.Parse(strarr[0]);
                PointPick.Trans.Y = float.Parse(strarr[1]);
                PointPick.Trans.Z = float.Parse(strarr[2]);
                PointPick.Rot.Q1 = float.Parse(strarr[3]);
                PointPick.Rot.Q2 = float.Parse(strarr[4]);
                PointPick.Rot.Q3 = float.Parse(strarr[5]);
                PointPick.Rot.Q4 = float.Parse(strarr[6]);

                PointPick_up.Trans.X = float.Parse(strarr[7]);
                PointPick_up.Trans.Y = float.Parse(strarr[8]);
                PointPick_up.Trans.Z = float.Parse(strarr[9]);
                PointPick_up.Rot.Q1 = float.Parse(strarr[10]);
                PointPick_up.Rot.Q2 = float.Parse(strarr[11]);
                PointPick_up.Rot.Q3 = float.Parse(strarr[12]);
                PointPick_up.Rot.Q4 = float.Parse(strarr[13]);

                PointPick_upup.Trans.X = float.Parse(strarr[14]);
                PointPick_upup.Trans.Y = float.Parse(strarr[15]);
                PointPick_upup.Trans.Z = float.Parse(strarr[16]);
                PointPick_upup.Rot.Q1 = float.Parse(strarr[17]);
                PointPick_upup.Rot.Q2 = float.Parse(strarr[18]);
                PointPick_upup.Rot.Q3 = float.Parse(strarr[19]);
                PointPick_upup.Rot.Q4 = float.Parse(strarr[20]);

                PointPut.Trans.X = float.Parse(strarr[21]);
                PointPut.Trans.Y = float.Parse(strarr[22]);
                PointPut.Trans.Z = float.Parse(strarr[23]);
                PointPut.Rot.Q1 = float.Parse(strarr[24]);
                PointPut.Rot.Q2 = float.Parse(strarr[25]);
                PointPut.Rot.Q3 = float.Parse(strarr[26]);
                PointPut.Rot.Q4 = float.Parse(strarr[27]);

                PointPut_up.Trans.X = float.Parse(strarr[28]);
                PointPut_up.Trans.Y = float.Parse(strarr[29]);
                PointPut_up.Trans.Z = float.Parse(strarr[30]);
                PointPut_up.Rot.Q1 = float.Parse(strarr[31]);
                PointPut_up.Rot.Q2 = float.Parse(strarr[32]);
                PointPut_up.Rot.Q3 = float.Parse(strarr[33]);
                PointPut_up.Rot.Q4 = float.Parse(strarr[34]);

                PointPut_upup.Trans.X = float.Parse(strarr[35]);
                PointPut_upup.Trans.Y = float.Parse(strarr[36]);
                PointPut_upup.Trans.Z = float.Parse(strarr[37]);
                PointPut_upup.Rot.Q1 = float.Parse(strarr[38]);
                PointPut_upup.Rot.Q2 = float.Parse(strarr[39]);
                PointPut_upup.Rot.Q3 = float.Parse(strarr[40]);
                PointPut_upup.Rot.Q4 = float.Parse(strarr[41]);
                #endregion

                for (int i = 0; i < reCount; i++)
                {
                    try
                    {
                        using (Mastership m = Mastership.Request(controller.Rapid))
                        {
                            RapidData pick = controller.Rapid.GetRapidData("T_ROB1", "pick");
                            pick.Value = PointPick;

                            RapidData pick_z_up = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up");
                            pick_z_up.Value = PointPick_up;

                            RapidData pick_z_upup = controller.Rapid.GetRapidData("T_ROB1", "pick_z_up_hight");
                            pick_z_upup.Value = PointPick_upup;

                            RapidData picka = controller.Rapid.GetRapidData("T_ROB1", "put");
                            picka.Value = PointPut;

                            RapidData pick_z_upa = controller.Rapid.GetRapidData("T_ROB1", "pre_put");
                            pick_z_upa.Value = PointPut_up;

                            RapidData pick_z_upaup = controller.Rapid.GetRapidData("T_ROB1", "pre_put_hight");
                            pick_z_upaup.Value = PointPut_upup;

                            RapidData Flgs = controller.Rapid.GetRapidData("T_ROB1", "VISION_STATE");//
                            Flgs.Value = new Num(Convert.ToInt32(strarr[43]));

                            RapidData Flg = controller.Rapid.GetRapidData("T_ROB1", "plug_state");//
                            Flg.Value = new Num(Convert.ToInt32(strarr[42]));

                            RapidData Hg = controller.Rapid.GetRapidData("T_ROB1", "START");//写
                            Hg.Value = new Bool(false);

                            lbs.Text = DateTime.Now.TimeOfDay.ToString() + "__发送";
                        }
                        SetString("发送成功");
                        break;
                    }
                    catch{ SetString("重复发送"+ i.ToString()); }
                    Thread.Sleep(10);
                }

            }
            GetBool = false;
            SendComd.BackColor = Color.Green;
        }

        public void RunStatue(string str)
        {
            if (!connectFlg)
            {
                MessageBox.Show("机器人故障无法连接");
                return;
            }
            //string[] strarr = str.Split(',');
            //if (strarr.Length != 5)
            //{
            //    return;
            //}

            if (str.Length!=1)
            {
                return; //plug_state
            }


            try
            {
                using (Mastership m = Mastership.Request(controller.Rapid))
                {
                    RapidData Flg = controller.Rapid.GetRapidData("T_ROB1", "plug_state");//
                    Flg.Value = new Num(Convert.ToInt32(str));
                    //    RapidData VISION_PICTRUE_NG = controller.Rapid.GetRapidData("T_ROB1", "VISION_PICTRUE_NG");
                    //    RapidData VISION_PICTRUE_OK = controller.Rapid.GetRapidData("T_ROB1", "VISION_PICTRUE_OK");
                    //    RapidData VISION_OUT_RANGE = controller.Rapid.GetRapidData("T_ROB1", "VISION_OUT_RANGE");
                    //    RapidData VISION_SYSTEM_ERROR = controller.Rapid.GetRapidData("T_ROB1", "VISION_SYSTEM_ERROR");

                    //    RapidData plug_state = controller.Rapid.GetRapidData("T_ROB1", "plug_state");
                    //
                }
             }
            catch
            {
                MessageBox.Show("向机器人写入数据失败");
            }
        }




        ControllerInfo[] controllers;
        Controller controller;
        /// <summary>
        /// 连接机器人控制器
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void button1_Click(object sender, EventArgs e)
        {
           
            //scanner.Scan();
            ListViewItem item = null;

            if (vir.Checked)
            {
                //连接虚拟
                controllers = scanner.GetControllers(NetworkScannerSearchCriterias.Virtual);
            }
            else
            {
                //连接机器人
                controllers = scanner.GetControllers();
            }
            foreach (ControllerInfo controllerInfo in controllers)
            {
                item = new ListViewItem(controllerInfo.IPAddress.ToString());
                item.SubItems.Add(controllerInfo.Id);
                item.SubItems.Add(controllerInfo.Availability.ToString());
                item.SubItems.Add(controllerInfo.IsVirtual.ToString());
                item.SubItems.Add(controllerInfo.SystemName);
                item.SubItems.Add(controllerInfo.Version.ToString());
                item.SubItems.Add(controllerInfo.ControllerName);
                this.listView1.Items.Add(item);
                item.Tag = controllerInfo;
            }
            if (controllers.Length>0)//连接机器人
            {
                if (controller != null)
                {
                    controller.Logoff();
                    controller.Dispose();
                    controller = null;
                }
                controller = ControllerFactory.CreateFrom(controllers[0]);
                controller.Logon(UserInfo.DefaultUser);
                controller.OperatingModeChanged += new EventHandler<OperatingModeChangeEventArgs>(OperatingModeChanged);	//mode change event  模式切换事件
                connectFlg = true;
            }
        }

        //上线扫描事件
        void HandleFoundEvent(object sender, NetworkWatcherEventArgs e)
        {
            Online = true;
            ListViewItem item = null;
            if (vir.Checked)
            {
                //连接虚拟
                controllers = scanner.GetControllers(NetworkScannerSearchCriterias.Virtual);
            }
            else
            {
                //连接机器人
                controllers = scanner.GetControllers();
            }
            foreach (ControllerInfo controllerInfo in controllers)
            {
                item = new ListViewItem(controllerInfo.IPAddress.ToString());
                item.SubItems.Add(controllerInfo.Id);
                item.SubItems.Add(controllerInfo.Availability.ToString());
                item.SubItems.Add(controllerInfo.IsVirtual.ToString());
                item.SubItems.Add(controllerInfo.SystemName);
                item.SubItems.Add(controllerInfo.Version.ToString());
                item.SubItems.Add(controllerInfo.ControllerName);
                this.listView1.Items.Add(item);
                item.Tag = controllerInfo;
            }
            if (controllers.Length > 0)//连接机器人
            {
                if (controller != null)
                {
                    controller.Logoff();
                    controller.Dispose();
                    controller = null;
                }
                controller = ControllerFactory.CreateFrom(controllers[0]);
                while (Online)
                {
                    try
                    {                     
                        controller.Logon(UserInfo.DefaultUser);
                        controller.OperatingModeChanged += new EventHandler<OperatingModeChangeEventArgs>(OperatingModeChanged);    //mode change event  模式切换事件
                        connectFlg = true;
                        break;
                    }
                    catch 
                    {

                    }
                }
            }

        }
 

        //掉线扫描事件
        void HandleLostEvent(object sender, NetworkWatcherEventArgs e)
        {
            Online = false;
            connectFlg = false;
            this.listView1.Items.Clear();
            if (controller != null)
            {
                controller.Logoff();
                controller.Dispose();
                controller = null;
            }
        }



        bool autoFlg = false;
        bool connectFlg = false;
        // 显示运行模式状态
        void OperatingModeChanged(object sender, OperatingModeChangeEventArgs e)
        {
            // this.Invoke(new EventHandler<OperatingModeChangeEventArgs>(ModeIsChanged), new Object[] { this, e });
            string str = e.NewMode.ToString();
            label6.Text = str;
            if (str == ControllerOperatingMode.Auto.ToString())
            {
                autoFlg = true;

            }
            //else if (str == ControllerOperatingMode.AutoChange.ToString())
            //{

            //}
            //else if (str == ControllerOperatingMode.Init.ToString())
            //{

            //}
            //else if (str == ControllerOperatingMode.ManualFullSpeed.ToString())
            //{

            //}
            //else if (str == ControllerOperatingMode.ManualFullSpeedChange.ToString())
            //{

            //}
            //else if (str == ControllerOperatingMode.ManualReducedSpeed.ToString())
            //{

            //}
            else
            {
                autoFlg = false;
            }
        }
        private void Form1_Load(object sender, EventArgs e)
        {

        }

        /// <summary>
        /// 读取控制器中reg的值
        /// </summary>
        /// <param name="controller">选中控制器</param>
        /// <param name="reg">选中reg</param>
        /// <param name="tb">读取显示的文本箱</param>
        private void ReadRapidRegNum(RapidData rd, Controller controller, string reg, TextBox tb)
        {
            rd = controller.Rapid.GetRapidData("T_ROB1", "user", reg);
            ABB.Robotics.Controllers.RapidDomain.Num rdreg = (ABB.Robotics.Controllers.RapidDomain.Num)rd.Value;
            tb.Text = rdreg.ToString();
        }


        /// <summary>
        /// 写入reg的值到控制器中
        /// </summary>
        /// <param name="rd">rapid数据</param>
        /// <param name="controller">所要写入的控制器</param>
        /// <param name="reg">所要写入的reg</param>
        /// <param name="tb">读取写入的文本箱</param>
        private void WriteRapidRegNum(RapidData rd, Controller controller, string reg, TextBox tb)
        {
            using (Mastership m = Mastership.Request(controller.Rapid))
            {
                rd = controller.Rapid.GetRapidData("T_ROB1", "user", reg);
                ABB.Robotics.Controllers.RapidDomain.Num rdreg = (ABB.Robotics.Controllers.RapidDomain.Num)rd.Value;
                rdreg.FillFromString2(tb.Text);
                rd.Value = rdreg;
            }
        }

        private void Form1_Load_1(object sender, EventArgs e)
        {

        }

        /// <summary>
        /// 读取控制器信号
        /// </summary>
        /// <param name="controller"></param>
        private void ReadSignal(Controller controller)
        {
            //Signal do1, di1;
            //do1 = controller.IOSystem.GetSignal("do00");
            //di1 = controller.IOSystem.GetSignal("di00");
            //btnDo1.Text = do1.Value.ToString();
            //btnDi1.Text = di1.Value.ToString();
        }

        /// <summary>
        /// 回原点
        /// </summary>
        /// <param name="sender"></param>
        /// <param name="e"></param>
        private void btnGoHome_Click(object sender, EventArgs e)
        {

            ABB.Robotics.Controllers.RapidDomain.RobTarget jCurPos = controller.MotionSystem.ActiveMechanicalUnit.GetPosition(CoordinateSystemType.Base);
            MessageBox.Show(jCurPos.Trans.Z.ToString());
        }

        private bool JointtargetJudge(ABB.Robotics.Controllers.RapidDomain.JointTarget jointTarget1, ABB.Robotics.Controllers.RapidDomain.JointTarget jointTarget2)
        {
            bool result = false;
            int a = 0;
            if (Math.Abs(jointTarget1.RobAx.Rax_1 - jointTarget2.RobAx.Rax_1) <= 0.0001)
            {
                a++;
            }
            if (Math.Abs(jointTarget1.RobAx.Rax_2 - jointTarget2.RobAx.Rax_2) <= 0.0001)
            {
                a++;
            }
            if (Math.Abs(jointTarget1.RobAx.Rax_3 - jointTarget2.RobAx.Rax_3) <= 0.0001)
            {
                a++;
            }
            if (Math.Abs(jointTarget1.RobAx.Rax_4 - jointTarget2.RobAx.Rax_4) <= 0.0001)
            {
                a++;
            }
            if (Math.Abs(jointTarget1.RobAx.Rax_5 - jointTarget2.RobAx.Rax_5) <= 0.0001)
            {
                a++;
            }
            if (Math.Abs(jointTarget1.RobAx.Rax_6 - jointTarget2.RobAx.Rax_6) <= 0.0001)
            {
                a++;
            }
            if (a == 6)
            {
                result = true;
            }
            return result;
        }

        private bool RobtargetJudge(ABB.Robotics.Controllers.RapidDomain.RobTarget robTarget1, ABB.Robotics.Controllers.RapidDomain.RobTarget robTarget2)
        {
            bool result = false;
            int a = 0;
            if(Math.Abs(robTarget1.Trans.X-robTarget2.Trans.X)<= 0.1)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Trans.Y - robTarget2.Trans.Y) <= 0.1)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Trans.Z - robTarget2.Trans.Z) <= 0.1)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Rot.Q1 - robTarget2.Rot.Q1) <= 0.001)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Rot.Q2 - robTarget2.Rot.Q2) <= 0.001)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Rot.Q3 - robTarget2.Rot.Q3) <= 0.001)
            {
                a++;
            }
            if (Math.Abs(robTarget1.Rot.Q4 - robTarget2.Rot.Q4) <= 0.001)
            {
                a++;
            }
            if (a == 7)
            {
                result = true;
            }
            return result;
        }




        /// <summary>
        /// 模拟仿真
        /// </summary>
        bool sdds = false;
        private void button4_Click(object sender, EventArgs e)
        {

            for (int i = 0; i < 100000; i++)
            {
                if (sdds)
                {
                    sv.SendStr("get" + "," + "1" + "," + "2");
                    sdds = false;
                }
                else
                {
                    sv.SendStr("get" + "," + "2" + "," + "2");
                    sdds = true;
                }
                Thread.Sleep(1000);
            }
        }


    }

    class learn
    {

        Controller controller;
        private void test()
        {
            //创建自定义数据
            RapidDataType rdt;
            rdt = this.controller.Rapid.GetRapidDataType("T_ROB1", "user",
            "processdata");
            UserDefined processdata = new UserDefined(rdt);


            //UserDefined processdata = new UserDefined(rdt);
            //processdata = (UserDefined)rd.Value;
            string no1 = processdata.Components[0].ToString();
            string no2 = processdata.Components[1].ToString();
        }
        private void test1()
        {

        }
    }



    class Server
    {
        private TcpListener tcpListener;
        private Thread listenThread;
        public TcpClient client;
        public ABB机器人 MW;
        NetworkStream clientStream;
        bool runflg = false;

        public Server()
        {
            this.tcpListener = new TcpListener(IPAddress.Any, 500);
            this.listenThread = new Thread(new ThreadStart(ListenForClients));
            this.listenThread.IsBackground = true;
            this.listenThread.Start();
        }

        public void SendStr(string str)
        {
            if (runflg)
            {
                try
                {
                    ASCIIEncoding encoder = new ASCIIEncoding();
                    byte[] buffer = encoder.GetBytes(str);
                    clientStream.Write(buffer, 0, buffer.Length);
                    clientStream.Flush();
                }
                catch 
                {

                    
                }
            }
        }
        private void ListenForClients()
        {
            this.tcpListener.Start();

            while (true)
            {
                client = this.tcpListener.AcceptTcpClient();
                Thread clientThread = new Thread(new ParameterizedThreadStart(HandleClientComm));
                clientThread.IsBackground = true;
                clientThread.Start(client);
            }
        }


        private void HandleClientComm(object client)
        {
            TcpClient tcpClient = (TcpClient)client;
            clientStream = tcpClient.GetStream();

            byte[] message = new byte[4096];
            int bytesRead = 0;
            runflg = true;
            while (runflg)
            {
                bytesRead = 0;

                try
                {
                    bytesRead = clientStream.Read(message, 0, 4096);
                }
                catch
                {
                    //MessageBox.Show("网络断开");
                    break;
                }
                if (bytesRead == 0)//连接断开
                {
                    runflg = false;
                    break;
                }
                else
                {
                    //解析接收内容
                    ASCIIEncoding encoder = new ASCIIEncoding();
                    string recvstr = encoder.GetString(message, 0, bytesRead);
                    if (recvstr=="point")
                    {
                        //发送
                        byte[] buffer = encoder.GetBytes(MW.ret(recvstr));
                        //byte[] buffer = encoder.GetBytes("0,0,0,1,0,0,0");

                        clientStream.Write(buffer, 0, buffer.Length);
                        clientStream.Flush();
                    }
                    if (recvstr.Length>10)
                    {
                        MW.RunPoint(recvstr);
                    }
                    if (recvstr.Length==1)
                    {
                        MW.RunStatue(recvstr);
                    }
                }
            }
            tcpClient.Close();
        }
    }
}
